#include "ros/ros.h"
#include "serial/serial.h"
#include <string>
#include <vector>
#include <iostream>
#include <ctime>
#include <ros/package.h>
#include <std_msgs/Bool.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <mavros_msgs/State.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/ExtendedState.h>
#include <fstream>
#include "tf/transform_datatypes.h"
#include <eigen3/Eigen/Core>
using namespace std;

enum air_state{
    in_air,
    landing,
    landed,
    takeoffing,
    waitting_takeoff
};

enum arm_state{
    armed,
    disarmed
};

enum fly_mode{
    offboard,
    manual,
    altctl,
    posctl,
    stabilized
};

enum control_methord{
    pos,
    vel,
    att
};

enum position_state{
    using_vicon,
    using_gps,
    using_att,
};

enum run_state{
    run_takeoff,
    run_traj,
    run_hold_traj_end,
    run_hold,
    run_land
};

struct current_state{
    arm_state armState;
    air_state airState;
    control_methord controlMethord;
    position_state positionState;
    fly_mode flyMode;
    run_state currentRunState;
    run_state nextRunState;
    bool current_run_finished;
};


// >>>>>>>>>>>>>>> 全局坐标系 使用ENU <<<<<<<<<
#define PI 3.141593


// >>>>>>>>>>>>>>> 全局变量 <<<<<<<<<<<<<<<<
string node_name;
vector<vector<float>> traj;// [i][0-3]: timestamp, x, y, z
vector<vector<float>> resampled_traj;
string readfilepath;
string recordfilename;
string recordfilepath;
string mavros_id;
string pkg_path;
string local_pose_topic_name;
string publish_pose_topic_name;

mavros_msgs::PositionTarget sp_pos;
current_state state;
tf::Quaternion cur_att;
ros::Publisher sp_raw_local_pub;
ros::Publisher current_drone_ready_pub;
ros::ServiceClient cmd_request;
mavros_msgs::SetMode mode_cmd;
mavros_msgs::CommandBool arm_cmd;
mavros_msgs::ExtendedState land_state;
std_msgs::Bool current_drone_ready_msg;
ros::ServiceClient arming_client;


float cur_pos[3]={0.0,0.0,0.0};
float record_pos[3]={0.0,0.0,0.0};
float cur_vel[3]={0.0,0.0,0.0};
float cur_angle[3]={0.0,0.0,0.0}; // roll p
float init_pose[3]={0.0,0.0,0.0};
float sp_rpyt[4]={0.0,0.0,0.0,0.0};
float yaw_rate = 0;
float takeoff_height = 0;
float land_height = 0;
float global_init_pose_x = 0;
float global_init_pose_y = 0;
float land_speed = 0.3;
float rect_x = 1;  //矩形x边长
float rect_y = 1;  //矩形y边长
float traj_scale = 1;
float traj_scale_x= 1;
float traj_scale_y = 1;
float traj_scale_z = 1;
float cur_yaw = 0.0;
float target_height = 0.0;
float _target[3] = {0.0,0.0,0.0};
float offset_z = 5.0;
float offset_y = 0.0;
float cur_err_dist = 0.0;


bool init_process_finished = false;
bool take_off_finished = false;
bool land_finished = false;
bool record_data = false;
bool fly_traj = true;
bool traj_finished = false;
bool fly_finished = false;
bool using_gazebo = false;
bool out_send_global_target = false;
bool use_traj_z = false;
bool use_resample = false;
bool use_takeoff = true;
bool use_land = true;
bool drone0_ready = true;
bool drone1_ready = true;
bool drone2_ready = true;
bool drone3_ready = true;


int use_enu = 1;//int 1 use_enu 0 use ned
int re_pub_pos = 0;//int 1 use_enu 0 use ned
int using_position_methord = 0;//int 1 using_vicon 2 using_gps 3 using_att
int using_control_mode = 0; // 0 position 1 vel 2 att
int id = 0;
int rate = 30;
int move_stage = 0;
int control_count_time = 0;
int control_time = 60;
int traj_speed = 1;
int traj_speed_control = 1;
int resample_num = 0;



// >>>>>>>>>>>>>>> 函数声明 <<<<<<<<<<<<<<<<
bool readCSV(const string& filename, vector<vector<float>> &traj);
void updateParams(ros::NodeHandle &nh);
void chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg);
void chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg);
void chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg);
void chatterCallback_mode(const mavros_msgs::State &msg);
void takeoff(float height);
void holdTakeOffPos(float height);
void land();
void holdZeroPosition();
void initState();
void load_traj();
void moveToRelativeNow(float dx, float dy, float dz);
void moveto(float target_x,float target_y, float target_z);
void moveto(float target_x,float target_y, float target_z, int i,int total);
inline float getDistancefrom(float x,float y,float z);
void fly_traj_count_base(float base_x,float base_y,float base_z,int _control_count);
void fly_traj_base(float base_x,float base_y,float base_z);
// 把traj的轨迹点数由原来的size个减小为num个 num应该和size 差不多大
void resample(int num);
void fly_rect();
void init_position(ros::Rate &loop_rate);

void drone0_ready_cb(const std_msgs::Bool &msg);
void drone1_ready_cb(const std_msgs::Bool &msg);
void drone2_ready_cb(const std_msgs::Bool &msg);
void drone3_ready_cb(const std_msgs::Bool &msg);

void setDIsarmed();
void setArmed();
void setAltctl();
void setStabilized();
void setPosctl();
void setOffboard();
void record_pose();
bool swarm_ready();
template <typename T>
T readParam(ros::NodeHandle &n, std::string name);

// >>>>>>>>>>>>>>> 全局参数 <<<<<<<<<<<<<<<<

int main(int argc ,char ** argv){
    ros::init(argc,argv,"offb_core");
    ros::NodeHandle n;
    node_name = ros::this_node::getName();
    pkg_path = ros::package::getPath("px4_gcs");

    updateParams(n);
    initState();
    load_traj();
    sp_raw_local_pub = n.advertise<mavros_msgs::PositionTarget>(
            mavros_id+"/setpoint_raw/local", 1);
    current_drone_ready_pub = n.advertise<std_msgs::Bool>(
            mavros_id+"/ready", 1);
    cmd_request = n.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
    arming_client = n.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    ros::Subscriber local_pose_sub = n.subscribe(
            mavros_id+"/local_position/pose", 1,chatterCallback_local_pose);
    ros::Subscriber vision_pose_sub = n.subscribe(
            mavros_id+"/vision_pose/pose", 1,chatterCallback_local_pose);
    ros::Subscriber local_vel_sub = n.subscribe(
            mavros_id+"/local_position/velocity", 1,chatterCallback_local_vel);
    ros::Subscriber mode_sub = n.subscribe(
            mavros_id+"/state", 1,chatterCallback_mode);
    ros::Subscriber lan_sub = n.subscribe(
            mavros_id+"/extended_state", 1,chatterCallback_extend_state);

    ros::Subscriber drone0_sub = n.subscribe(
            "/uav0/mavros0/ready", 1,drone0_ready_cb);
    ros::Subscriber drone1_sub = n.subscribe(
            "/uav1/mavros1/ready", 1,drone1_ready_cb);
    ros::Subscriber drone2_sub = n.subscribe(
            "/uav2/mavros2/ready", 1,drone2_ready_cb);
    ros::Subscriber drone3_sub = n.subscribe(
            "/uav3/mavros3/ready", 1,drone3_ready_cb);
    
    ros::Rate loop_rate(rate);
    ros::spinOnce();
    if(use_resample){
        ROS_INFO_STREAM("resample data origin data size "<< traj.size()<<"sampled data size:"<< resample_num);
        resample(resample_num);
    }
    while (ros::ok()){
        if(!init_process_finished){
            //初始化位置等信息.
            // init_position(loop_rate);
            ROS_INFO("init position.......");
            ros::spinOnce(); //获取回调消息
            init_pose[0] = 0.0;
            init_pose[1] = 0.0;
            init_pose[2] = 0.0;
            for (int j = 0; j <30; ++j) {
                init_pose[0] += cur_pos[0];
                init_pose[1] += cur_pos[1];
                init_pose[2] += cur_pos[2];
                loop_rate.sleep(); //控制帧率
                ros::spinOnce(); //获取回调消息
            }
            init_pose[0] = init_pose[0]/20;
            init_pose[1] = init_pose[1]/20;
            init_pose[2] = init_pose[2]/20;
            init_process_finished = true;
            ROS_INFO_STREAM("init position "<<init_pose[0]<<" "<<init_pose[1]<<" "<<init_pose[1]<<" ");
            ROS_INFO("init position finished READY TAKEOFF!!!!");
            if(using_gazebo){
                holdZeroPosition();
                setOffboard();
                setArmed();
            }

        }
        ros::spinOnce(); //获取回调消息
        if(state.flyMode!=offboard){
            holdZeroPosition();
        }else{
            if(use_takeoff){
                takeoff(takeoff_height);
            }
            else{
                take_off_finished = true;
            }

            current_drone_ready_msg.data = take_off_finished;
            current_drone_ready_pub.publish(current_drone_ready_msg);

            if(swarm_ready()){
                if(take_off_finished&&!fly_traj){
                    fly_rect();
                }
                if(take_off_finished&&fly_traj&&!traj_finished){
                    fly_traj_base(init_pose[0],init_pose[1],takeoff_height);
                }
                if(traj_finished&&!fly_finished){
                    fly_traj_count_base(init_pose[0],init_pose[1],takeoff_height,(int)traj.size()-1);// 飞向航迹末端点悬停
                    if(cur_err_dist<0.3){
                        record_pose();
                        fly_finished = true;
                    }
                }
                // 飞行完轨迹后悬停或者降落
                if(fly_finished){
                    if(use_land){
                        land();
                        if(state.armState==disarmed){
                            ROS_INFO("Land FINISHED!! set stabilized mode disarmed!!");
                            break;
                        }
                    }else{
                        fly_traj_count_base(init_pose[0],init_pose[1],takeoff_height,(int)traj.size()-1);// 飞向航迹末端点悬停
                        // moveto(record_pos[0],record_pos[1],land_height);
                    }
                }
            }else{
                holdTakeOffPos(takeoff_height);
                record_pose();
            }
        }
        loop_rate.sleep();
    }
}

void process_run(){
    switch (state.currentRunState){
        case run_takeoff:
            state.nextRunState = run_hold;
            break;
        case run_hold :
            state.nextRunState = run_traj;
            break;

        case run_traj:
            state.nextRunState = run_hold_traj_end;
            break;

        case run_hold_traj_end:
            moveto(record_pos[0],record_pos[1],land_height);
            state.nextRunState = use_land?run_land:run_hold_traj_end;
            break;

        case run_land:
            if(use_land){
                land();
                if(state.armState==disarmed){
                    ROS_INFO("Land FINISHED!! set stabilized mode disarmed!!");
                    break;
                }
            }else{
                moveto(record_pos[0],record_pos[1],land_height);
            }
            break;
        default:
            break;
    }
}
void fly_traj_count_base(float base_x,float base_y,float base_z,int _control_count){
    if(!use_resample){
        _target[0] = traj_scale_x*traj[_control_count][0+3*id]+base_x;
        _target[1] = traj_scale_y*traj[_control_count][1+3*id]+base_y;
        if(use_traj_z) _target[2] = traj_scale_z*traj[_control_count][2+3*id]+base_z;
        else _target[2] = takeoff_height;
    }else{
        _target[0] = traj_scale_x*resampled_traj[_control_count][0+3*id]+base_x;
        _target[1] = traj_scale_y*resampled_traj[_control_count][1+3*id]+base_y;
        if(use_traj_z) _target[2] = traj_scale_z*resampled_traj[_control_count][2+3*id]+base_z;
        else _target[2] = takeoff_height;
    }
    moveto(_target[0],_target[1],_target[2]);
}


void fly_traj_base(float base_x,float base_y,float base_z){
    ROS_INFO_STREAM("FLY_TRAJ......"<<(float)control_count_time*100/traj.size()<<" %");
    if(control_count_time<traj.size()){
        fly_traj_count_base(base_x,base_y,base_z,control_count_time);
        if(traj_speed>=0){
            if(traj_speed == 0)traj_speed = 1;
            for (int j = 0; j < traj_speed; ++j) {
                control_count_time++;
            }
        }else{
            traj_speed_control++;
            if(traj_speed_control+traj_speed>=0){
                traj_speed_control = 1;
                control_count_time++;
            }
        }
    }else{
        traj_finished = true;
        control_count_time = 0;
        ROS_INFO("TRAJ FINISHED!!");
        record_pose();
    }
}


void fly_rect(){
    switch(move_stage){
        case 0:
            if(control_count_time<control_time){
                moveto(-1*rect_x/2,-1*rect_y/2,takeoff_height);
                control_count_time++;
            }else{
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly traj 2");
            }

            break;
        case 1:
            if(control_count_time<control_time){
                moveto(-1*rect_x/2,rect_y/2,takeoff_height);
                control_count_time++;
            }else{
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly traj 3");

            }
            break;
        case 2:
            if(control_count_time<control_time){
                moveto(rect_x/2,rect_y/2,takeoff_height);
                control_count_time++;
            }else{
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly traj 4");

            }
            break;
        case 3:
            if(control_count_time<control_time){
                moveto(rect_x/2,-1*rect_y/2,takeoff_height);
                control_count_time++;
            }else{
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("Now fly back");
            }
            break;
        case 4:
            if(control_count_time<control_time){
                moveto(init_pose[0],init_pose[1],takeoff_height);
                control_count_time++;
            }else{
                record_pose();
                control_count_time = 0;
                move_stage++;
                ROS_INFO("TRAJ FINISHED!!");
            }
            break;
        default:
            fly_finished = true;
            traj_finished = true;
            break;
    }
}

bool readCSV(const string& filename, vector<vector<float>> &traj)
{
    ifstream traj_file(filename);
    if(traj_file.fail()){
        ROS_INFO_STREAM("can not find this file"<<filename);
        return false;
    }

    string line;
    while(getline(traj_file, line)){
        istringstream in_line(line);
        vector<float> dataInLine;
        string word;
        while(getline(in_line, word, ',')){
            dataInLine.push_back(strtof(word.c_str(), nullptr));
        }
        if(!dataInLine.empty()) traj.push_back(dataInLine);
    }

    if(!traj.empty()){
        return true;
    }else{
        ROS_INFO_STREAM("Found nothing in the file"<<filename);
        return false;
    }
}

template <typename T>
T readParam(ros::NodeHandle &n, std::string name)
{
    T ans;
    name = node_name+"/"+name;
    if (n.getParam(name, ans))
    {
        ROS_INFO_STREAM("Loaded " << name << ": " << ans);
    }
    else
    {
        ROS_ERROR_STREAM("Failed to load " << name);
        n.shutdown();
    }
    return ans;
}

void updateParams(ros::NodeHandle &nh)
{
    id = readParam<int>(nh,"id");
    rate = readParam<int>(nh,"rate");
    traj_speed = readParam<int>(nh,"traj_speed");
    resample_num = readParam<int>(nh,"resample_num");
    global_init_pose_y = readParam<float>(nh,"global_init_pose_y");
    global_init_pose_x = readParam<float>(nh,"global_init_pose_x");
    takeoff_height = readParam<float>(nh,"takeoff_height");
    land_height = readParam<float>(nh,"land_height");
    land_speed = readParam<float>(nh,"land_speed");
    offset_y = readParam<float>(nh,"offset_y");
    mavros_id = readParam<string>(nh,"mavros_id");
    readfilepath = readParam<string>(nh,"readfilepath");
    using_position_methord = readParam<int>(nh,"using_position_methord");
    using_gazebo = readParam<bool>(nh,"using_gazebo");
    fly_traj = readParam<bool>(nh,"fly_traj");
    use_resample = readParam<bool>(nh,"use_resample");
    use_traj_z = readParam<bool>(nh,"use_traj_z");
    use_takeoff = readParam<bool>(nh,"use_takeoff");
    use_land = readParam<bool>(nh,"use_land");
    traj_scale = readParam<float>(nh,"traj_scale");
    traj_scale_x = readParam<float>(nh,"traj_scale_x");
    traj_scale_y = readParam<float>(nh,"traj_scale_y");
    traj_scale_z = readParam<float>(nh,"traj_scale_z");
    out_send_global_target = readParam<bool>(nh,"out_send_global_target");
    ROS_INFO_STREAM("mavros_id: "+mavros_id);
}

void chatterCallback_local_pose(const geometry_msgs::PoseStamped &msg)
{
    cur_pos[0] = msg.pose.position.x;
    cur_pos[1] = msg.pose.position.y;
    cur_pos[2] = msg.pose.position.z;

    cur_att.setX(msg.pose.orientation.x);
    cur_att.setY(msg.pose.orientation.y);
    cur_att.setZ(msg.pose.orientation.z);
    cur_att.setW(msg.pose.orientation.w);

    cur_angle[2] = (float)(atan2(2*cur_att.getY()*cur_att.getX() - 2*cur_att.getZ()*cur_att.getW(),
                                 -2*cur_att.getY()*cur_att.getY() - 2*cur_att.getW()*cur_att.getW() +1 ) + PI);
    cur_angle[1] = (float)(asin(2*cur_att.getZ()*cur_att.getX()-2*cur_att.getY()*cur_att.getW()));
    cur_angle[0] = (float)(atan2(2*cur_att.getX()*cur_att.getW()+2*cur_att.getY()*cur_att.getZ(),
                                 1-2*cur_att.getY()*cur_att.getY()-2*cur_att.getX()*cur_att.getX()));
}


void drone0_ready_cb(const std_msgs::Bool &msg){
    drone0_ready = msg.data;
}
void drone1_ready_cb(const std_msgs::Bool &msg){
    drone1_ready = msg.data;
}
void drone2_ready_cb(const std_msgs::Bool &msg){
    drone2_ready = msg.data;
}
void drone3_ready_cb(const std_msgs::Bool &msg){
    drone3_ready = msg.data;
}

void chatterCallback_local_vel(const geometry_msgs::TwistStamped &msg)
{
    cur_vel[0] = msg.twist.linear.x;
    cur_vel[1] = msg.twist.linear.y;
    cur_vel[2] = msg.twist.linear.z;
    yaw_rate = msg.twist.angular.z;
}

void chatterCallback_mode(const mavros_msgs::State &msg)
{
    if(msg.armed==1){
        state.armState = armed;
    }else{
        state.armState = disarmed;
    }
    if(msg.mode=="OFFBOARD"){
        state.flyMode = offboard;
    }else if(msg.mode=="STABILIZED"){
        state.flyMode = stabilized;
    }else if(msg.mode=="ALTCTL"){
        state.flyMode = altctl;
    }else if(msg.mode=="POSCTL"){
        state.flyMode = posctl;
    }
}


void takeoff(float height)
{
    if(!take_off_finished){
        state.airState = takeoffing;
        if(fabs(cur_pos[2]-height)<0.1){
            take_off_finished = true;
            return;
        }
        moveto(init_pose[0],init_pose[1],height);
    }
}

void holdTakeOffPos(float height){
    moveto(init_pose[0],init_pose[1],height);
}

void land()
{
    if(state.airState==landed){
        if(state.armState==armed){
            setStabilized();
            setDIsarmed();
        }
    }
    if(!land_finished){
        ROS_INFO("landing now");
        state.airState = landing;
        //
        if((cur_pos[2]>-5||state.airState == landing)&&(state.airState != landed)){
            moveto(record_pos[0],record_pos[1]+offset_y,cur_pos[2]-land_speed);
        } else{
            land_finished = true;
            state.airState = landed;
        }
    }
}




void holdZeroPosition()
{
    moveToRelativeNow(0,0,0);
}

void moveto(float target_x,float target_y, float target_z)
{
    sp_pos.header.frame_id = "map";
    if(using_control_mode == 0){
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.position.x = target_x;
        sp_pos.position.y = target_y;
        sp_pos.position.z = target_z;
        sp_pos.type_mask = 0b110111111000;  // 100 111 111 000  xyz + yaw
        sp_pos.coordinate_frame = 1;
        sp_raw_local_pub.publish(sp_pos);
    }else if (using_control_mode == 1){
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.velocity.x = target_x;
        sp_pos.velocity.y = target_y;
        sp_pos.velocity.z = target_z;
        sp_pos.type_mask = 0b110111000111;  // 100 111 111 000  xyz + yaw
        sp_pos.coordinate_frame = 1;
        sp_raw_local_pub.publish(sp_pos);
    }
    cur_err_dist = getDistancefrom(_target[0],_target[1],_target[2]);
}

void moveto(float target_x,float target_y, float target_z, int i,int total)
{
    i = i+1;
    float index = (float)(i)/(float)(total);
    moveto(cur_pos[0]+index*(target_x-cur_pos[0]),
           cur_pos[1]+index*(target_y-cur_pos[1]),
           cur_pos[2]+index*(target_z-cur_pos[2])
            );
}

void moveToRelativeNow(float dx, float dy, float dz){
    sp_pos.header.frame_id = "map";
    if(using_control_mode == 0){
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.position.x = cur_pos[0]+dx;
        sp_pos.position.y = cur_pos[1]+dy;
        sp_pos.position.z = cur_pos[2]+dz;
        sp_pos.type_mask = 0b110111111000;  // 100 111 111 000  xyz + yaw
        sp_pos.coordinate_frame = 1;
        sp_raw_local_pub.publish(sp_pos);
        ROS_INFO("published moving to : %2.4f, %2.4f, %2.4f",
                                                      sp_pos.position.x,
                                                      sp_pos.position.y,
                                                      sp_pos.position.z);
    }else if (using_control_mode == 1){
        sp_pos.header.stamp = ros::Time::now();
        sp_pos.velocity.x = dx;
        sp_pos.velocity.y = dy;
        sp_pos.velocity.z = dz;
        sp_pos.type_mask = 0b110111000111;  // 100 111 111 000  xyz + yaw
        sp_pos.coordinate_frame = 1;
        sp_raw_local_pub.publish(sp_pos);
    }

}

void load_traj()
{
    readfilepath = pkg_path+readfilepath;
    readCSV(readfilepath,traj);
    ROS_INFO_STREAM("load traj success  "<<readfilepath);
    if(out_send_global_target){
        for(auto & i : traj){
            i[0+3*id]-=global_init_pose_x;
            i[1+3*id]-=global_init_pose_y;
        }
    }
}

void initState(){
    switch (using_position_methord){
        case 1:
            state.positionState = using_vicon;
            break;
        case 2:
            state.positionState = using_gps;
            break;
        case 3:
            state.positionState = using_att;
            break;
        default:
            break;
    }
}

void setOffboard()
{
    mode_cmd.request.custom_mode = "OFFBOARD";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting OFFBOARD ");
}

void setMannual()
{
    mode_cmd.request.custom_mode = "MANUAL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting MANNUAL ");
}

void setPosctl()
{
    mode_cmd.request.custom_mode = "POSCTL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting POSCTL ");
}

void setAltctl()
{
    mode_cmd.request.custom_mode = "ALTCTL";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting ALTCTL ");
}

void setStabilized()
{
    mode_cmd.request.custom_mode = "STABILIZED";
    cmd_request.call(mode_cmd);
    ROS_INFO("setting STABILIZED ");
}

void setArmed()
{
    arm_cmd.request.value = true;
    arming_client.call(arm_cmd);
    ROS_INFO("setting Armed ");
}

void setDIsarmed()
{
    arm_cmd.request.value = false;
    arming_client.call(arm_cmd);
    ROS_INFO("setting Disarmed ");
}

void record_pose()
{
    record_pos[0] = cur_pos[0];
    record_pos[1] = cur_pos[1];
    record_pos[2] = cur_pos[2];
}

void chatterCallback_extend_state(const mavros_msgs::ExtendedState &msg)
{
    if(msg.landed_state==1){
        state.airState=landed;
    }else{
        state.airState=in_air;
    }
}

// 把traj的轨迹点数由原来的size个减小为num个 num应该和size 差不多大
void resample(int num)
{
    resampled_traj.resize(num);
    int size = traj.size();
    // num 应该和size 差不多大
    if(size>num){
        int set_num = (size-2)/(size-num);
        int mod_num = (size-2)%(size-num);
        int delet_num = size-num;
        int rindex = 0;
        int index = 0;
        resampled_traj[rindex++] = traj[index++];
        for (int j = 0; j < mod_num/2; ++j) {
            resampled_traj[rindex++] = traj[index++];
        }

        for (int k = 0; k < delet_num; ++k) {
            for (int j = 0; j < set_num-1; ++j) {
                resampled_traj[rindex++] = traj[index++];
            }
            index++;
        }
        for (int i = 0; i < mod_num-mod_num/2; ++i) {
            resampled_traj[rindex++] = traj[index++];
        }
        resampled_traj[num-1] = traj[size-1];
    }
}

inline float getDistancefrom(float x,float y,float z)
{
    return sqrt((cur_pos[0]-x)*(cur_pos[0]-x)+
                        (cur_pos[1]-y)*(cur_pos[1]-y)+
                        (cur_pos[2]-z)*(cur_pos[2]-z));
}

bool swarm_ready(){
    return drone0_ready&&drone1_ready&&drone2_ready&&drone3_ready;
}

void init_position(ros::Rate &loop_rate){
    ROS_INFO("init position.......");
    ros::spinOnce(); //获取回调消息
    init_pose[0] = 0.0;
    init_pose[1] = 0.0;
    init_pose[2] = 0.0;
    for (int j = 0; j <20; ++j) {
        init_pose[0] += cur_pos[0];
        init_pose[1] += cur_pos[1];
        init_pose[2] += cur_pos[2];
        loop_rate.sleep(); //控制帧率
        ros::spinOnce(); //获取回调消息
    }
    init_pose[0] = init_pose[0]/20;
    init_pose[1] = init_pose[1]/20;
    init_pose[2] = init_pose[2]/20;
    init_process_finished = true;
    ROS_INFO_STREAM("init position "<<init_pose[0]<<" "<<init_pose[1]<<" "<<init_pose[1]<<" ");
    ROS_INFO("init position finished READY TAKEOFF!!!!");
}


